#include "./PORT/port.h"
#include "../System/config.h"

#include "./BSP/TB6612/tb6612.h"
#include "./BSP/MOTOR/motor.h"
#include "./BSP/ENCODER/encoder.h"

uint16_t motor_dead_band_speed[] = {CFG_MOTOR_DEAD_BAND};

/**
 * @brief  初始化电机
 * @param  None
 * @retval None
 */
void motor_init(void)
{
#if CFG_MOTOR_DRIVER == 1
    tb6612_init();
#elif CFG_MOTOR_DRIVER == 2

#endif
}

/**
 * @brief  电机初始化
 * @param  motor_id: 电机编号
 * @param  direction: 方向 0:停止 1:正转 2:反转
 * @retval None
 */
void motor_set_direction(uint8_t motor_id, uint8_t direction)
{
#if CFG_MOTOR_DRIVER == 1
    tb6612_set_direction(motor_id, direction);
#elif CFG_MOTOR_DRIVER == 2

#endif
}

/**
 * @brief  设置电机PWM值
 * @param  motor_id: 电机编号
 * @param  pwm_value: PWM值
 * @retval None
 */
void motor_set_pwm_value(uint8_t motor_id, uint16_t pwm_value)
{
#if CFG_MOTOR_DRIVER == 1
    tb6612_set_pwm_value(motor_id, pwm_value);
#elif CFG_MOTOR_DRIVER == 2

#endif
}

/**
 * @brief  设置电机速度
 * @param  motor_id: 电机编号
 * @param  speed: 速度
 * @retval None
 */
void motor_set_speed(uint8_t motor_id, int16_t speed)
{
    if (speed > 0)
    {
        motor_set_direction(motor_id, CFG_MOTOR_DIRECTION_FORWARD);
        motor_set_pwm_value(motor_id, speed + motor_dead_band_speed[motor_id]);
    }
    else if (speed < 0)
    {
        motor_set_direction(motor_id, CFG_MOTOR_DIRECTION_BACKWARD);
        motor_set_pwm_value(motor_id, -speed - motor_dead_band_speed[motor_id]);
    }
    else
    {
        motor_set_direction(motor_id, CFG_MOTOR_DIRECTION_STOP);
        motor_set_pwm_value(motor_id, speed);
    }
}

/**
 * @brief  获取电机速度
 * @param  encoder_id: 编码器编号
 * @param  time_ms: 时间
 * @retval 速度 单位: cm/s
 */
float motor_get_speed(uint8_t encoder_id, uint16_t time_ms)
{
    float speed = 0;
    speed = (float)encoder_get_value(encoder_id) / CFG_MOTOR_PPR * CFG_MOTOR_WHEEL_DIAMETER_CM / time_ms * 1000.0f;
    return speed;
}

/**
 * @brief  获取电机已走过距离
 * @param  encoder_id: 编码器编号
 * @retval 行驶的距离 单位: cm
 */
float motor_get_now_distance(uint8_t encoder_id)
{
    float now_cm = 0;
    now_cm = (float)encoder_get_total_count(encoder_id) / CFG_MOTOR_PPR * CFG_MOTOR_WHEEL_DIAMETER_CM ;
    return now_cm;
}
